#include "bsp_pid.h"
#include "string.h"
#include "math.h"
#include "main.h"
#define PI 3.1415926
pid_struct_t pid_temp_struct;

void pit_init(void){
    memcpy(&pid_temp_struct,&temp_pid_init_val,sizeof(pid_struct_t));
}

/**
  * @brief  pid reset
  * @param  pid    the pid struct containing parameters
  * @return none
  */
void pid_reset(pid_struct_t * pid){
	pid->err[1] = pid->err[0] = 0.0f;
	pid->i_out = 0.0f;
}

/**
  * @brief  pid calculation
  * @param  pid    the pid struct containing parameters
  * @param  ref    reference value
  * @param  cur    current value
  * @return pid calculation result
  * @note pid->k_deadband is invaild in this process
  */
float pid_calc(pid_struct_t *pid, float ref, float cur){
  float output;
  pid->err[1] = pid->err[0];
  pid->err[0] = ref - cur;
  
  pid->p_out  = pid->kp * pid->err[0];
  pid->i_out += pid->ki * pid->err[0];
  pid->d_out  = pid->kd * (pid->err[0] - pid->err[1]);
  LIMIT_MIN_MAX(pid->i_out, -pid->i_max, pid->i_max);
	if(isnan(pid->i_out))pid->i_out = 0;
  
  output = pid->p_out + pid->i_out + pid->d_out;
  LIMIT_MIN_MAX(output, -pid->out_max,0);
  return output;
}

const pid_struct_t temp_pid_init_val={
	.kp = 0.10f,
	.ki = 0.0f,     
	.kd = 0.10f,      
	.i_max = v_DATA_MAX/50,
	.out_max = v_DATA_MAX,
	.k_deadband = 500
};

